//
// Created by geoffrey on 24-12-15.
//

#include "controller.h"

controller::controller(Matrix<double, 6, 2> state) {
    state_d << 0, 2, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0;
    State_d.push_back(state_d);
    state_error << (state(0,0) - state_d(0,0)) * cos(state(5,0)) * cos(state(4,0))
                  +(state(1,0) - state_d(1,0)) * sin(state(5,0)) * cos(state(4,0))
                  -(state(2,0) - state_d(2,0)) * sin(state(4,0)) ,0,

                  -(state(0,0) - state_d(0,0)) * sin(state(3,0))
                  +(state(1,0) - state_d(1,0)) * cos(state(3,0)) ,0,

                   (state(0,0) - state_d(0,0)) * cos(state(5,0)) * sin(state(4,0))
                  +(state(1,0) - state_d(1,0)) * sin(state(5,0)) * sin(state(4,0))
                  +(state(2,0) - state_d(2,0)) * cos(state(4,0)) ,0,

                   0,0,

                   (state(4,0) - state_d(4,0)),0,
                   (state(5,0) - state_d(5,0)),0;
    tau<<200,0,0;
}

void controller::update(Matrix<double, 6, 2> state, Matrix<double, 6, 1> f) {
    //double  m_u = 6.245698224525000e+03 , m_q = 2.605095684768638e+04 , m_r = 1.523785538173125e+04 ;
    double  m_u = 40 , m_q = 1000 , m_r = 800 ;
    double	u = state(0,1), v = state(1, 1), w = state(2,1), p = state(3,1), q = state(4,1), r = state(5,1);
    double 	x = state(0,0), y = state(1, 0), z = state(2, 0),phi = state(3,0), theta = state(4,0), psi = state(5,0);
    double	u_d = state_d(0,1), v_d = state_d(1, 1), w_d = state_d(2,1), p_d = state_d(3,1), q_d = state_d(4,1), r_d = state_d(5,1);
    double 	x_d = state_d(0,0), y_d = state_d(1, 0), z_d = state_d(2, 0),phi_d = state_d(3,0), theta_d = state_d(4,0), psi_d = state_d(5,0);
    double 	x_1 = u_d * cos(psi_d) * cos(theta_d) + v_d * (cos(psi_d) * sin(theta_d) * sin(phi_d) - sin(psi_d) * cos(phi_d)) + w_d * (sin(psi_d) * sin(phi_d) + cos(psi_d) * cos(phi_d) * sin(theta_d)),
            y_1 = u_d * sin(psi_d) * cos(theta_d) + v_d * (cos(psi_d) * cos(phi_d) + sin(psi_d) * sin(theta_d) * sin(phi_d)) + w_d * (sin(psi_d) * sin(theta_d) * cos(phi_d) - cos(psi_d) * sin(phi_d)),
            z_1 = -u_d * sin(theta_d) + v_d * cos(theta_d) * sin(phi_d) + w_d * cos(theta_d) * cos(phi_d),
            phi_1 = p_d + q_d * sin(phi_d) * tan(theta_d) + r_d * cos(phi_d) * tan(theta_d),
            theta_1 = q_d * cos(phi_d) - r_d * sin(phi_d),
            psi_1 = q_d * sin(phi_d) / cos(theta_d) + r_d * cos(phi_d) / cos(theta_d);
    Matrix<double , 6, 1> d_state_d;
    d_state_d<< x_1 , y_1 , z_1 , phi_1 , theta_1 , psi_1;
    if (t<150)
        state_d << state_d.block(0,0,6,1) + d_state_d*h , state_d.block(0,1,6,1);
    else {
        Matrix<double , 6, 1> state_d_150;
        state_d_150 << 2, 0, 0, 0, 0, 0.025 * (sin(0.006*t) - cos(0.006*t));
        state_d << state_d.block(0,0,6,1) + d_state_d*h, state_d_150 ;
        //cout << state_d<<endl;
    }
    State_d.push_back(state_d);
    state_error <<  (x - x_d) * cos(psi) * cos(theta) + (y - y_d) * sin(psi) * cos(theta) - (z - z_d) * sin(theta),u-u_d,
                   -(x - x_d) * sin(psi) + (y - y_d) * cos(psi),v-v_d,
                    (x - x_d) * cos(psi) * sin(theta) + (y - y_d) * sin(psi) * sin(theta) + (z - z_d) * cos(theta),w-w_d,
                    0,0,
                    theta - theta_d,q-q_d,
                    psi - psi_d,r-r_d;

    double	u_e = state_error(0,1), v_e = state_error(1, 1), w_e = state_error(2,1), p_e = state_error(3,1), q_e = state_error(4,1), r_e = state_error(5,1);
    double 	x_e = state_error(0,0), y_e = state_error(1, 0), z_e = state_error(2, 0),phi_e = state_error(3,0), theta_e = state_error(4,0), psi_e = state_error(5,0);


    double h_x1 = 2 * (u_d * sin(psi_e / 2) + w_d * cos(psi_e / 2));
    double h_x2 = -2 * (v_d * cos(psi_e / 2) - u_d * sin(psi_e / 2)) * cos(theta_e);
    double h_y = 2 * (u_d * cos(psi_e / 2) + v_d * sin(psi_e / 2));
    double h_z = -2 * (u_d * cos(psi_e) * cos(psi_e / 2) +
                       v_d * sin(psi_e) * cos(psi_e / 2) -
                       w_d * sin(psi_e / 2));
    // 计算 alpha_u
    double E = sqrt(1 + pow(x_e,2) + pow(y_e,2) + pow(z_e,2));
    double alpha_u = -k_x * (x_e / E);
    // 计算 alpha_q
    double alpha_q = -k_theta * sin(theta_e / 2) - gamma_q * h_z * (z_e / E);
    // 计算 alpha_r
    double alpha_r = (-k_psi * sin(psi_e / 2) - gamma_r * h_y * (y_e / E)) * cos(theta) +
                     r_d * (1 - cos(theta) / cos(theta_d));

    Matrix<double, 3, 1> d_bar_alpha ;
    d_bar_alpha << (alpha_u - d_bar_alpha(0))/T_alpha, (alpha_q - d_bar_alpha(1))/T_alpha, (alpha_r - d_bar_alpha(2))/T_alpha;
    bar_alpha+=d_bar_alpha*h;

    //double bar_alpha_u = bar_alpha(0);
    //double bar_alpha_q = bar_alpha(1);
    //double bar_alpha_r = bar_alpha(2);

    double bar_alpha_u = alpha_u;
    double bar_alpha_q = alpha_q;
    double bar_alpha_r = alpha_r;

    double error_bar_u = u_e - bar_alpha_u;
    double error_bar_q = q_e - bar_alpha_q;
    double error_bar_r = r_e - bar_alpha_r;

    // 计算 d_bar_tau_i
    double bar_tau_prime_u = -k_u * error_bar_u - f(0) - tau_W_i_M * tanh(lambda_0 * error_bar_u) + m_u * (bar_alpha_u - alpha_u_old)/h;
    double d_bar_tau_u = lambda_chi1 * (bar_tau_prime_u - bar_tau(0));
    // 计算 d_tau_i
    double N_u = (exp(lambda_chi1 * chi(0) * chi(0)) * cos(chi(0)));
    double zeta_u = (k_zeta1 * (bar_tau(0) - g(tau(0),tau_u_M)) + d_bar_tau_u + ((g(tau(0),tau_u_M) - g(tau(0)-h,tau_u_M))/h) * k_alpha * tau(0));
    double d_tau_u = -k_alpha * tau(0) + N_u * zeta_u;
    // 计算 d_chi_i
    double d_chi_u = (1 / k_chi1) * zeta_u * (bar_tau(0) - g(tau(0),tau_u_M));

    // 计算 d_bar_tau_i
    double bar_tau_prime_q = -k_q * error_bar_q - f(4) - tau_W_i_M * tanh(lambda_0 * error_bar_q) + m_q * (bar_alpha_q - alpha_q_old)/h;
    double d_bar_tau_q = lambda_chi2 * (bar_tau_prime_q - bar_tau(1));
    // 计算 d_tau_i
    double N_q = (exp(lambda_chi2 * chi(1) * chi(1)) * cos(chi(1)));
    double zeta_q = (k_zeta2 * (bar_tau(1) - g(tau(1),tau_q_M)) + d_bar_tau_q + ((g(tau(1),tau_q_M) - g(tau(1)-h,tau_q_M))/h) * k_alpha * tau(1));
    double d_tau_q = -k_alpha * tau(1) + N_q * zeta_q;
    // 计算 d_chi_i
    double d_chi_q = (1 / k_chi2) * zeta_q * (bar_tau(1) - g(tau(1),tau_q_M));

    // 计算 d_bar_tau_i
    double bar_tau_prime_r = -k_r * error_bar_r - f(5) - tau_W_i_M * tanh(lambda_0 * error_bar_r) + m_r * (bar_alpha_r - alpha_r_old)/h;
    double d_bar_tau_r = lambda_chi3 * (bar_tau_prime_r - bar_tau(2));
    // 计算 d_tau_i
    double N_r = (exp(lambda_chi3 * chi(2) * chi(2)) * cos(chi(2)));
    double zeta_r = (k_zeta3 * (bar_tau(2) - g(tau(2),tau_r_M)) + d_bar_tau_r + ((g(tau(2),tau_r_M) - g(tau(2)-h,tau_r_M))/h) * k_alpha * tau(2));
    double d_tau_r = -k_alpha * tau(2) + N_r * zeta_r;
    // 计算 d_chi_i
    double d_chi_r = (1 / k_chi3) * zeta_r * (bar_tau(2) - g(tau(2),tau_r_M));

    alpha_u_old = bar_alpha_u;
    alpha_q_old = bar_alpha_q;
    alpha_r_old = bar_alpha_r;



    //更新
    Matrix<double , 3, 1> d_tau , d_bar_tau , d_chi;
    d_tau << d_tau_u , d_tau_q , d_tau_r ;
    d_bar_tau << d_bar_tau_u , d_bar_tau_q , d_bar_tau_r ;
    d_chi << d_chi_u , d_chi_q , d_chi_r ;
    tau+=d_tau*h;
    bar_tau+=d_bar_tau*h;
    chi+=d_chi*h;

    cout<<t<<endl<<"tau = "<<endl<<tau<<endl;
    //<<"state_error = "<<endl<<error_bar_u<<endl;
    t+=h;
}

Matrix<double, 6, 1> controller::getTau() {

    Matrix<double, 6, 1> tau_all;
    tau_all <<g(tau(0),tau_u_M), 0, 0, 0, g(tau(1),tau_q_M),g(tau(2),tau_r_M);
    return tau_all;

}

vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> controller::getState_d() {
    return State_d;
}